//
// Created by xiang on 2022/7/18.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "ch7/direct_ndt_lo.h"
#include "ch7/ndt_3d.h"
#include "common/dataset_type.h"
#include "common/io_utils.h"
#include "common/timer/timer.h"
#include "tools/ui/pangolin_window.h"
/// 本程序以ULHK数据集为例
/// 测试以NDT为主的Lidar Odometry
/// 若使用PCL NDT的话，会重新建立NDT树
//DEFINE_string(bag_path, "./dataset/sad/ulhk/test2.bag", "path to rosbag");
//DEFINE_string(dataset_type, "ULHK", "NCLT/ULHK/KITTI/WXB_3D");  // 数据集类型

DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
//DEFINE_string(pcd_path, "../data/ch7/map.pcd", "点云文件路径");
DEFINE_string(pcd_path, "", "点云文件路径");
DEFINE_bool(use_pcl_ndt, false, "use pcl ndt to align?");
DEFINE_bool(use_ndt_nearby_6, false, "use ndt nearby 6?");
DEFINE_bool(display_map, false, "display map?");

using PointType = pcl::PointXYZI;
using PointCloudType = pcl::PointCloud<PointType>;

int main(int argc, char** argv) {
    if (!FLAGS_pcd_path.empty()) {
        // 读取点云
        PointCloudType::Ptr cloud(new PointCloudType);
        pcl::io::loadPCDFile(FLAGS_pcd_path, *cloud);

        if (cloud->empty()) {
            LOG(ERROR) << "cannot load cloud file";
            return -1;
        }
        // visualize
        pcl::visualization::PCLVisualizer viewer("map cloud viewer");
        pcl::visualization::PointCloudColorHandlerGenericField<PointType> handle(cloud, "z");  // 使用高度来着色
        viewer.addPointCloud<PointType>(cloud, handle);
        viewer.spin();
    }

    ui::PangolinWindow ui;
    if (ui.Init() == false) {
        return -1;
    }

    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    sad::DirectNDTLO::Options options;
    options.use_pcl_ndt_ = fLB::FLAGS_use_pcl_ndt;
    options.ndt3d_options_.nearby_type_ =
        FLAGS_use_ndt_nearby_6 ? sad::Ndt3d::NearbyType::NEARBY6 : sad::Ndt3d::NearbyType::CENTER;
    options.display_realtime_cloud_ = FLAGS_display_map;
    sad::DirectNDTLO ndt_lo(options);

    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    Scan2dPtr last_scan = nullptr, current_scan = nullptr;
    io.SetPointCloud3DProcessFunc([&](const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_ptr) -> bool  {
            common::Timer::Evaluate(
                [&]() {
                    SE3 pose;
                    SE3 predict_pose;
                    float voxel_size = 0.1;
                    ndt_lo.AddCloud(sad::VoxelCloud(cloud_ptr, voxel_size), pose, predict_pose);
                    if(ui.ShouldQuit() == false) {
                        //ui.UpdateNavState(sad::NavStated(0, pose));
                        ui.UpdateScan(ndt_lo.local_map(), pose);
                        //ui.UpdateScan(sad::VoxelCloud(cloud_ptr, voxel_size), SE3(SO3(), Eigen::Matrix<double, 3, 1>::Zero()));
                        
                        //ui.UpdateScan(sad::VoxelCloud(cloud_ptr, voxel_size), ndt_lo.world_pose());
                    }
                },
                "NDT registration");
            return true;
    })
    .Go();

    if (FLAGS_display_map) {
        // 把地图存下来
        ndt_lo.SaveMap("../data/ch7/map.pcd");
    }

    common::Timer::PrintAll();
    LOG(INFO) << "done.";

    ui.Quit();

    return 0;
}
